import numpy as np
import tf.transformations as tf

def deg2rad(deg):
    return deg * np.pi / 180. 

def rad2deg(rad):
    return rad / np.pi * 180. 

# 定义欧拉角 (roll, pitch, yaw)
roll = 0.1
pitch = 0.2
yaw = 0.3

# 动轴-zyx
R = tf.euler_matrix(yaw, pitch, roll, axes='rzyx')
# 将旋转矩阵转换为四元数
quat = tf.quaternion_from_matrix(R)
# 打印四元数 (注意顺序是 [x, y, z, w])
print(quat)

# static静坐标系(外旋-固定轴)  rotating动坐标系(内旋-动轴)
eu = tf.euler_from_matrix(R, axes='sxyz')
print(rad2deg(eu[0]), rad2deg(eu[1]), rad2deg(eu[2]))

eu = tf.euler_from_matrix(R, axes='rzyx')
print(rad2deg(eu[2]), rad2deg(eu[1]), rad2deg(eu[0]))

# 四元素转欧拉角
eu = tf.euler_from_quaternion(quat, axes='rzyx')
print(rad2deg(eu[2]), rad2deg(eu[1]), rad2deg(eu[0]))